/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      AddFunction.c
 * @author    Fortiortech  Appliction Team
 * @since     Create:2022-07-13
 * @date      Last modify:2022-07-14
 * @note      Last modify author is Marcel He
 * @brief     This file contains main function used for Motor Control.
 */

#include "application.h"
#include "separator.h"

#include <MyProject.h>

bool data               isCtrlPowOn = false; // 开关机控制
PWMINPUTCAL xdata       mcPwmInput;          // PWM捕获结构体变量
foc_ctrl_t xdata        foc_control;         // FOC电机控制相关结构体变量
motor_ctrl_ramp_t xdata motor_ref_ramp;      // 控制指令爬坡结构体相关变量
debugONOFFTypeDef xdata debug_ONOFFTest;     // ONOFF启停测试小工具结构体变量

extern separator_t  separator;
extern work_state_t main_state;

/**
 *   @brief      对变量取16位的绝对值
 *   @param[in]  value
 *   @return     绝对值
 *   @date       2022-07-13
 */
uint16 abs_int16(int16 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/**
 *  @brief      对变量取32位的绝对值
 *  @param[in]  value
 *  @return     绝对值
 *  @date       2022-07-13
 */
uint32 abs_int32(int32 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/**
 * @brief   调速型号给定，设置固定转速
 */
void set_target_ref(void)
{
    isCtrlPowOn     = true;       // 开机
    foc_control.ref = I_Value(5); // 固定转速赋值

    //	main_state = driect_control;
    //	separator.can_data.target_speed = 8000;
    //	if (main_state == driect_control)
    //	{
    //		isCtrlPowOn = true;
    //		foc_control.ref = S_Value(separator.can_data.target_speed);
    //	}
}

/**
 * @brief   外部闭环控制函数，示例代码提供 电流环，速度环，功率环，UQ控制示例代码,可根据需要自行修改
 *          建议使用默认1ms周期运行
 * @date    2022-07-14
 */
void speed_response(void)
{
    static int16 refRampOut       = 0;
    static uint8 Flag_Single_Mode = 0;

    if ((motor_ctrl_state == run) || (motor_ctrl_state == stop))
    {
        switch (foc_control.control_mode)
        {
        case 0:
            if (foc_control.speed_filtered > MOTOR_LOOP_RPM)
            {
                foc_control.mode_0_hold_count++;
                if (foc_control.mode_0_hold_count > 50)
                {
                    foc_control.control_mode = 1;
                    FOC_QKP                  = QKP;
                    FOC_QKI                  = QKI;
                    FOC_DKP                  = DKP;
                    FOC_DKI                  = DKI;

#if (MOTOR_CTRL_MODE == SPEED_LOOP_CONTROL)
                    // 启动电流环与外环给定衔接
                    motor_ref_ramp.out_value_float = foc_control.speed_filtered; // 速度环
#endif

                    FOC_THECOMP                   = _Q15(0.0 / 180.0);
                    foc_control.loop_time         = LOOP_TIME;
                    motor_ref_ramp.increase_value = RAMP_INC;
                    motor_ref_ramp.decrease_value = RAMP_DEC;

                    foc_control.iq_ref = FOC_IQREF;
                    FOC_IDREF          = ID_RUN_CURRENT;

                    PI1_UKH = foc_control.iq_ref;
                }
            }
            else
            {
                foc_control.angle_com = ((foc_control.ubus_filtered - UDC_Value(12.0)) >> 4) + _Q15(3.0 / 180.0);
                FOC_THECOMP           = foc_control.angle_com;
                foc_control.mode_0_hold_count = 0;
            }
            break;
        case 1:
            foc_control.loop_time++;
            if (foc_control.loop_time >= LOOP_TIME)
            {
                foc_control.loop_time = 0;
                refRampOut            = Motor_Ramp(foc_control.ref); // 控制命令爬坡函数，用于实现调速信号之间平滑过渡

#if (MOTOR_CTRL_MODE == CURRENT_LOOP_CONTROL)
                foc_control.iq_ref = refRampOut;
#elif (MOTOR_CTRL_MODE == SPEED_LOOP_CONTROL)
                foc_control.iq_ref = HW_One_PI(refRampOut - foc_control.speed_filtered);
#endif

                FOC_IQREF = foc_control.iq_ref;
            }
            break;
        }

#if (Single_Resistor_Mode == SVPWM_OPTIM)

        // 低速使用新单电阻采样，高速用传统单电阻采样
        if ((foc_control.speed_filtered > S_Value(1000.0)) && (Flag_Single_Mode == 0))
        {
            Flag_Single_Mode = 1;
            SetReg(FOC_CR1, CSM0 | CSM1, 0x00); // 单电阻采样配置
            FOC_TSMIN  = PWM_TS_LOAD1;          // 最小采样窗口
            FOC_TRGDLY = 0x0B;
        }
        else if ((foc_control.speed_filtered < S_Value(900.0)) && (Flag_Single_Mode == 1))
        {
            Flag_Single_Mode = 0;
            SetReg(FOC_CR1, CSM0 | CSM1, CSM1); // 新单电阻采样配置
            FOC_TSMIN  = PWM_TS_LOAD;           // 最小采样窗口
            FOC_TRGDLY = 0x07;
        }

#endif
    }
}

/**
 * @brief        控制给定爬坡函数
 *               以浮点进行计算，解决整数爬坡由于精度的影响，导致爬坡结果阶梯变化
 *               函数控制周期默认为闭环控制周期，建议使用默认1ms周期运行
 * @param[in]    ref 给定目标值
 * @return       爬坡结果（int16）
 * @date         2022-07-14
 */
int16 Motor_Ramp(int16 ref)
{
    motor_ref_ramp.ref_value = ref; // 爬坡函数输入

    if (motor_ref_ramp.out_value_float < motor_ref_ramp.ref_value)
    {
        if (motor_ref_ramp.out_value_float + motor_ref_ramp.increase_value < motor_ref_ramp.ref_value)
        {
            motor_ref_ramp.out_value_float += motor_ref_ramp.increase_value;
        }
        else
        {
            motor_ref_ramp.out_value_float = motor_ref_ramp.ref_value;
        }
    }
    else
    {
        if (motor_ref_ramp.out_value_float - motor_ref_ramp.decrease_value > motor_ref_ramp.ref_value)
        {
            motor_ref_ramp.out_value_float -= motor_ref_ramp.decrease_value;
        }
        else
        {
            motor_ref_ramp.out_value_float = motor_ref_ramp.ref_value;
        }
    }

    return (int16)motor_ref_ramp.out_value_float; // 输出浮点数取整
}

/**
 * @brief   启动ATO爬坡函数，用于静止启动时候对ATO进行爬坡，提高启动可靠性
 * @date    2022-07-14
 */
void ATORamp(void)
{
    if (motor_ctrl_state == run)
    {
        if (foc_control.control_mode == 0)
        {
            if (foc_control.iq_ref < IQ_Start_CURRENT) // 启动电流爬坡函数
            {
                foc_control.iq_ref += 30;
            }
            else
            {
                foc_control.iq_ref = IQ_Start_CURRENT;
            }

            FOC_IQREF = foc_control.iq_ref;
        }

        if (foc_control.motor_state_count == (ATO_RAMP_PERIOD << 2))
        {
            FOC_EKP = OBSW_KP_GAIN_RUN1; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN1; // 估算器里的PI的KI
        }
        else if (foc_control.motor_state_count == ((ATO_RAMP_PERIOD << 1) + ATO_RAMP_PERIOD))
        {
            FOC_EKLPFMIN = OBS_EA_KS2;
            FOC_EKP      = OBSW_KP_GAIN_RUN2; // 估算器里的PI的KP
            FOC_EKI      = OBSW_KI_GAIN_RUN2; // 估算器里的PI的KI
        }
        else if (foc_control.motor_state_count == (ATO_RAMP_PERIOD << 1))
        {
            FOC_EKLPFMIN = OBS_EA_KS;
            FOC_EKP      = OBSW_KP_GAIN_RUN3; // 估算器里的PI的KP
            FOC_EKI      = OBSW_KI_GAIN_RUN3; // 估算器里的PI的KI
        }
        else if ((foc_control.motor_state_count <= ATO_RAMP_PERIOD) && (foc_control.ato_ramp_end_flag == 0))
        {
            FOC_EKP                       = OBSW_KP_GAIN_RUN4; // 估算器里的PI的KP
            FOC_EKI                       = OBSW_KI_GAIN_RUN4; // 估算器里的PI的KI
            foc_control.ato_ramp_end_flag = 1;                 // ATO 爬坡结束
        }
    }
}

/**
 * @brief        默认1ms周期服务函数，运行信号采样，调速信号处理，闭环控制，故障检测,ATO爬坡函数
 *               该函数运行于大循环中，由SYSTICK定时器间隔1ms触发运行。
 * @date         2022-07-14
 */
void TickCycle_1ms(void)
{
    SetBit(ADC_CR, ADCBSY); // 使能ADC的DCBUS采样

    if ((motor_ctrl_state != init) && (motor_ctrl_state != ready))
    {
        /* 速度滤波 */
        foc_control.speed_filtered =
            LPFFunction(FOC__EOME, foc_control.speed_filtered, 50); // 注意低通滤波器系数范围为0---127
        foc_control.es_value = LPFFunction(FOC__EMF << 2, foc_control.es_value, 50);

        if (motor_ctrl_state == run)
        {
            foc_control.power          = FOC__POW << 1;
            foc_control.power_filtered = LPFFunction(foc_control.power, foc_control.power_filtered, 30); // 功率滤波
            foc_control.iq_filtered    = LPFFunction(FOC__IQ, foc_control.iq_filtered, 50);
        }
    }
    else
    {
        foc_control.speed_filtered = 0;
        foc_control.power_filtered = 0;
    }

    foc_control.ubus_filtered = LPFFunction(ADC15_DR, foc_control.ubus_filtered, 50);
    foc_control.uq_filtered   = LPFFunction(FOC__UQ, foc_control.uq_filtered, 50);
    foc_control.ud_filtered   = LPFFunction(FOC__UD, foc_control.ud_filtered, 50);

    foc_control.mcu_temperature = TSD_Gain(); // 获取芯片内部温度

    set_target_ref();  // 给定目标值
    Fault_Detection(); // 故障处理

    speed_response(); // 启动ATO控制，环路响应，如速度环、转矩环、功率环等

    ATORamp(); /* 电机启动ATO爬坡函数处理  */

    /* 电机状态机的时序处理 */
    if (foc_control.motor_state_count > 0)
    {
        foc_control.motor_state_count--;
    }
}
